#ifndef RADIATION_SOURCE_STATE_MACHINE__STATE_MACHINE_NODE_HPP_
#define RADIATION_SOURCE_STATE_MACHINE__STATE_MACHINE_NODE_HPP_

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/bool.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <memory>
#include <string>
#include <vector>
#include <map>
#include <mutex>
#include <thread>
#include <condition_variable>
#include <atomic>
#include <rclcpp/parameter_client.hpp>

#include "radiation_source_state_machine/lifecycle_node_client.hpp"


namespace radiation_source_state_machine
{

enum class SystemState {
    IDLE,
    SOURCE_DETECTION_FIRST,
    LOCAL_NAVIGATION,
    FIXED_MOVEMENT,
    MQTT_CLOUD_SUB_START,
    SOURCE_DETECTION_SECOND,
    FINE_ALIGNMENT,
    GRIPPER_CONTROL,
    COMPLETED,
    ERROR
};

class StateMachineNode : public rclcpp::Node
{
public:
    explicit StateMachineNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
    ~StateMachineNode();
    

    
    void startStateMachine();
    void stopStateMachine();
    void runStateMachine();
    void transitionToState(SystemState new_state);


    // 动态设置其他节点参数
    bool setNodeParameter(const std::string& node_name, const std::string& param_name, const rclcpp::ParameterValue& value);
    bool configureNodeParameters(const std::string& node_name);


    // 状态处理函数
    void handleIdleState();
    void handleSourceDetectionFirstState();
    void handleLocalNavigationState();
    void handleFixedMovementState();
    void handleMqttCloudSubStartState();
    void handleSourceDetectionSecondState();
    void handleFineAlignmentState();
    void handleGripperControlState();
    void handleCompletedState();
    void handleErrorState();


    // 回调函数
    void navigationReachedCallback(const std_msgs::msg::Bool::SharedPtr msg);
    void fixedMovementCompleteCallback(const std_msgs::msg::Bool::SharedPtr msg);
    void fineAlignmentCompleteCallback(const std_msgs::msg::Bool::SharedPtr msg);
    void gripperCompleteCallback(const std_msgs::msg::Bool::SharedPtr msg);
    void timerCallback();


    // 辅助函数
    bool startNode(const std::string& node_name);
    bool safeStopNode(const std::string& node_name);
    void publishTransientTopics();
    void cleanupStateResources();
    void publishTwistCommand(double linear_x, double angular_z, double duration_sec);
    void applyVelocitySmoothing(double& linear_x, double& linear_y, double& angular_z,
                                        double target_linear_x, double target_linear_y, double target_angular_z);
    double calculateMovementTime(double distance, double max_vel, double max_acc);

private:

     // 参数结构（从YAML加载）
    struct {
        std::string source_type;
        std::string action_type;
        double fixed_movement_distance;
        double fixed_movement_speed;
        double fixed_movement_duration;
        double node_activation_timeout;
        
        // 话题名称
        std::string navigation_reached_topic;
        std::string fixed_movement_complete_topic;
        std::string fine_alignment_completed_topic;
        std::string gripper_completed_topic;
        std::string cmd_vel_topic;
    } params_;


    // 速度json类实例
    VelocityJsonConverter vel_cmd; 
    // fixed_movement相关
    double target_linear_x;
    double target_linear_y; 
    double target_angular_z; 
    double prev_linear_x_;
    double prev_linear_y_;
    double prev_angular_z_;
    double max_acceleration_;
    double max_angular_acceleration_;


    // 状态变量
    std::atomic<SystemState> current_state_;
    std::atomic<SystemState> previous_state_;
    std::atomic<bool> state_machine_running_;
    std::mutex state_mutex_;
    std::condition_variable state_cv_;


    // 节点客户端
    std::map<std::string, std::shared_ptr<LifecycleNodeClient>> node_clients_;
    std::vector<std::string> managed_nodes_;

    // 参数客户端
    std::map<std::string, rclcpp::SyncParametersClient::SharedPtr> param_clients_;

    // 参数
    std::string source_type_;
    std::string action_type_;
    double fixed_movement_distance_;
    double fixed_movement_speed_;
    double fixed_movement_duration_;

    // 订阅和发布者
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr source_type_pub_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr action_type_pub_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr fixed_movement_complete_pub_;

    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr source_type_sub_;
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr action_type_sub_;
    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr navigation_reached_sub_;
    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr fixed_movement_complete_sub_;
    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr fine_alignment_complete_sub_;
    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr gripper_complete_sub_;

    // 定时器
    rclcpp::TimerBase::SharedPtr state_timer_;
    rclcpp::TimerBase::SharedPtr fixed_movement_timer_;

    // 状态完成标志位
    bool navigation_reached_;
    bool fixed_movement_complete_;
    bool fine_alignment_complete_;
    bool gripper_complete_;

    // Threads
    std::thread state_machine_thread_;
};

}  // namespace radiation_source_state_machine

#endif  // RADIATION_SOURCE_STATE_MACHINE__STATE_MACHINE_NODE_HPP_